#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_srvs.srv import Empty
from std_msgs.msg import Int32MultiArray
from inspire_hand.srv import set_pos
from darknet_ros_msgs.msg import BoundingBoxes

class ExampleMoveItTrajectories(object):
    """示例MoveIt轨迹"""
    def __init__(self):
        # 初始化节点
        super(ExampleMoveItTrajectories, self).__init__()
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('example_move_it_trajectories')

        rospy.loginfo("初始化灵巧手控制模块")

        self.set_position_client = rospy.ServiceProxy('inspire_hand/set_pos', set_pos)
        rospy.loginfo("等待灵巧手位置设置服务就绪")

        # 图像宽度设为1280
        self.image_width = 1280

        # 订阅 bounding_boxes 话题
        self.bounding_boxes_subscriber = rospy.Subscriber('/darknet_ros/bounding_boxes', BoundingBoxes, self.bounding_boxes_callback)
        rospy.loginfo("订阅 bounding_boxes 话题")

        try:
            # 检查是否存在夹爪
            self.is_gripper_present = rospy.get_param(rospy.get_namespace() + "is_gripper_present", False)
            if self.is_gripper_present:
                gripper_joint_names = rospy.get_param(rospy.get_namespace() + "gripper_joint_names", [])
                self.gripper_joint_name = gripper_joint_names[0]
            else:
                gripper_joint_name = ""
            self.degrees_of_freedom = rospy.get_param(rospy.get_namespace() + "degrees_of_freedom", 7)

            # 创建MoveIt接口所需的对象
            arm_group_name = "arm"
            self.robot = moveit_commander.RobotCommander("robot_description")
            self.scene = moveit_commander.PlanningSceneInterface(ns=rospy.get_namespace())
            self.arm_group = moveit_commander.MoveGroupCommander(arm_group_name, ns=rospy.get_namespace())
            self.display_trajectory_publisher = rospy.Publisher(rospy.get_namespace() + 'move_group/display_planned_path',
                                                        moveit_msgs.msg.DisplayTrajectory,
                                                        queue_size=20)

            if self.is_gripper_present:
                gripper_group_name = "gripper"
                self.gripper_group = moveit_commander.MoveGroupCommander(gripper_group_name, ns=rospy.get_namespace())

            rospy.loginfo("Initializing node in namespace " + rospy.get_namespace())
        except Exception as e:
            print (e)
            self.is_init_success = False
        else:
            self.is_init_success = True

    def reach_named_position(self, target):
        """达到命名位置"""
        arm_group = self.arm_group
        
        # 前往其中一个目标
        rospy.loginfo("Going to named target " + target)
        # 设置目标
        arm_group.set_named_target(target)
        # 计划轨迹
        planned_path1 = arm_group.plan()
        # 执行轨迹并阻塞，直到完成
        return arm_group.execute(planned_path1, wait=True)

    def reach_joint_angles(self, tolerance):
        """达到关节角度"""
        arm_group = self.arm_group
        success = True

        # 获取当前关节位置
        joint_positions = arm_group.get_current_joint_values()
        rospy.loginfo("Printing current joint positions before movement :")
        for p in joint_positions: rospy.loginfo(p)

        # 设置目标关节容差
        self.arm_group.set_goal_joint_tolerance(tolerance)

        # 设置关节目标配置
        if self.degrees_of_freedom == 7:
            joint_positions[0] = -pi/2
            joint_positions[1] = 0
            joint_positions[2] = -pi/4
            joint_positions[3] = pi/4
            joint_positions[4] = 0
            joint_positions[5] = -pi/2
            joint_positions[6] = 0.2
        elif self.degrees_of_freedom == 6:
            joint_positions[0] = 0
            joint_positions[1] = 0
            joint_positions[2] = -pi/2
            joint_positions[3] = -pi/4
            joint_positions[4] = 0
            joint_positions[5] = -pi/2
        arm_group.set_joint_value_target(joint_positions)

        # 规划并执行
        success &= arm_group.go(wait=True)

        # 显示移动后的关节位置
        new_joint_positions = arm_group.get_current_joint_values()
        rospy.loginfo("Printing current joint positions after movement :")
        for p in new_joint_positions: rospy.loginfo(p)
        return success

    def get_cartesian_pose(self):
        """获取笛卡尔姿态"""
        arm_group = self.arm_group

        # 获取当前姿态并显示
        pose = arm_group.get_current_pose()
        rospy.loginfo("Actual cartesian pose is : ")
        rospy.loginfo(pose.pose)

        return pose.pose

    def reach_cartesian_pose(self, pose, tolerance, constraints):
        """达到笛卡尔姿态"""
        arm_group = self.arm_group
        
        # 设置容差
        arm_group.set_goal_position_tolerance(tolerance)

        # 设置路径约束（如果有）
        if constraints is not None:
            arm_group.set_path_constraints(constraints)

        # 设置目标姿态
        arm_group.set_pose_target(pose)

        # 规划并执行
        rospy.loginfo("Planning and going to the Cartesian Pose")
        return arm_group.go(wait=True)
       
    def set_hand_position(self, grasp_angles_):
        try:
            # 将 Int32MultiArray 转换为 Python 列表
            grasp_angles = grasp_angles_.data

            # 调用服务代理函数，并传递参数列表
            response = self.set_position_client(*grasp_angles)
            if response.pos_accepted:
                rospy.loginfo("灵巧手位置设置成功!")
            else:
                rospy.logerr("灵巧手位置设置失败!")
        except rospy.ServiceException as e:
            rospy.logerr("调用服务失败: %s" % e)

    def control_single_joint(self, joint_index, joint_value):
        """控制单个关节"""
        arm_group = self.arm_group

        # 获取当前关节位置
        joint_positions = arm_group.get_current_joint_values()

        # 设置单个关节目标位置
        joint_positions[joint_index] = joint_value

        # 设置关节目标配置
        arm_group.set_joint_value_target(joint_positions)

        # 规划并执行
        success = arm_group.go(wait=True)

        # 显示移动后的关节位置
        new_joint_positions = arm_group.get_current_joint_values()
        rospy.loginfo("当前关节位置:")
        for p in new_joint_positions: rospy.loginfo(p)
        return success

        # Int32MultiArray(data=[73, 138, 135, 24, 700, 900])
        # 
        # 
        # 
        # 
        # 第五位数字指的是大拇指的弯曲度 0 是全开
        # 第六位数字指的是大拇指的旋转度 0 是平行于手背

    def get_banana(self, aim_):
        self.reach_named_position("home")    
        self.reach_named_position("position"+ str(aim_))
        self.reach_named_position("cube" + str(aim_))

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 抓香蕉
            grasp_angles = Int32MultiArray(data=[1200, 1200, 1200, 900, 858, 1745])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

        self.reach_named_position("position"+ str(aim_))
        self.reach_named_position("home")

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 释放
            grasp_angles = Int32MultiArray(data=[0, 0, 0, 0, 0, 0])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 虚握
            grasp_angles = Int32MultiArray(data=[73, 138, 135, 24, 1096, 1814])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

    def get_tennis(self, aim_):
        self.reach_named_position("home")    
        self.reach_named_position("position"+ str(aim_))
        self.reach_named_position("cube" + str(aim_))

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 抓网球
            grasp_angles = Int32MultiArray(data=[1200, 1100, 1000, 800, 1000, 2000])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

        self.reach_named_position("position"+ str(aim_))
        self.reach_named_position("home")

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 释放
            grasp_angles = Int32MultiArray(data=[0, 0, 0, 0, 0, 0])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 虚握
            grasp_angles = Int32MultiArray(data=[73, 138, 135, 24, 1096, 1814])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

    def get_bottle(self, aim_):
        self.reach_named_position("home")    
        self.reach_named_position("position"+ str(aim_))
        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 释放
            grasp_angles = Int32MultiArray(data=[0, 0, 0, 0, 0, 2000])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

        self.reach_named_position("cube" + str(aim_) +"_bottle")

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 抓罐子
            grasp_angles = Int32MultiArray(data=[1000, 1000, 1000, 1000, 1000, 2000])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

        self.reach_named_position("position"+ str(aim_))
        self.reach_named_position("home")

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 释放
            grasp_angles = Int32MultiArray(data=[0, 0, 0, 0, 0, 0])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 虚握
            grasp_angles = Int32MultiArray(data=[73, 138, 135, 24, 1096, 1814])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

    def get_apple(self, aim_):
        self.reach_named_position("home")    
        self.reach_named_position("position"+ str(aim_))
        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 释放
            grasp_angles = Int32MultiArray(data=[0, 0, 0, 0, 0, 2000])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

        self.reach_named_position("cube" + str(aim_) +"_bottle")

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 抓罐子
            grasp_angles = Int32MultiArray(data=[1000, 1000, 1000, 1000, 1000, 2000])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

        self.reach_named_position("position"+ str(aim_))
        self.reach_named_position("home")

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 释放
            grasp_angles = Int32MultiArray(data=[0, 0, 0, 0, 0, 0])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass

        try:
            # 创建一个Int32MultiArray消息对象，并设置抓取角度值 虚握
            grasp_angles = Int32MultiArray(data=[73, 138, 135, 24, 1096, 1814])
            self.set_hand_position(grasp_angles_ = grasp_angles)
        except rospy.ROSInterruptException:
            pass


    def bounding_boxes_callback(self, data):
        for box in data.bounding_boxes:
            if box.Class == 'banana':
                self.get_banana(aim_ = 1)
            if box.Class == 'tennis ball':
                self.get_tennis(aim_ = 2)


    def whole(self):
        # 接收两个话题信息
        # 一个话题是采集结果话题
        # 一个是boundingbox话题
        # 请求一个服务通信 GetState.srv 定义一个状态state
        # 如果状态state 正在采集 就等待 采集完成
        # 如果状态state 等待机械臂完成 就接收控制机械臂移动到目标位置,目标有采集结果话题来决定 并将状态state 修改为任务完成
        pass
        

if __name__ == '__main__':
    ex = ExampleMoveItTrajectories()
    ex.get_banana(6)
